"""
Redistribution and use in source and binary forms, with or
without modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.
* Neither the names of the authers, nor the names of its contributors, may be used to endorse or 
promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE REGENTS/COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' AND 
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
REGENTS/COPYRIGHT OWNERS AND CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

@Author Fredrique Samuels <fredriquesamuels@gmail.com>
@matrix.py  
"""


from math import *


#===========================================================

class Vector:
    """ 3 dimensional vector class """
    def __init__(self,x=0,y=0,z=0):
        """ Class constructor """
        self.x = x
        self.y = y
        self.z = z
        
    def set(self,x,y,z):
        self.x = x
        self.y = y
        self.z = z
        
    def __str__(self):
        ret = '<Vector '
        ret+='x='
        ret+=str(self.x)
        ret+=' y='
        ret+=str(self.y)
        ret+=' z='
        ret+=str(self.z)
        ret+=' >'
        return ret
    
    def copy(self):
        return Vector(self.x,self.y,self.z)
    
    def asTuple(self):
        return (self.x,self.y,self.z)
    
    def add(self,v):
        self.x += v.x
        self.y += v.y
        self.z += v.z
        
    def subtact(self,v):
        self.x -= v.x
        self.y -= v.y
        self.z -= v.z
        
    def mult(self,v):
        self.x *= v.x
        self.y *= v.y
        self.z *= v.z
        
    def div(self,v):
        self.x /= v.x
        self.y /= v.y
        self.z /= v.z
        
#=============================================================
        
def pytCrossProduct(v1,v2 ) :
    """ Calculate the cross pructs of 2 vectors

    @return Cross product""" 
    result = Vector()
    result.x = v1.y * v2.z - v1.z * v2.y
    result.y = v1.z * v2.x - v1.x * v2.z
    result.z = v1.x * v2.y - v1.y * v2.x
    return result

#===========================================================

def pytGetIdentityMatrix():
    """ Get a 4x4 matrix 
    
    @return: 4x4 Identity Matrix 
    """
    return [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]

#===========================================================


def pytTranslateMatrix(m,x,y,z):
    """ Translate matrix m by [x,y,z] 
    
    @param m: Target Matrix
    @param x: X value
    @param y: Y value
    @param z: Z value  
    """
    m[3][0] += x
    m[3][1] += y
    m[3][2] += z
        
#==============================================================

def pytGetRotationMatrixZ(d):
    """ Get the rotation matrix 
    for z degrees of rotation along
    the Z axis
    
    @param d: Degrees of rotation 
    @return: Rotation matrix
    """
    m = pytGetIdentityMatrix()
    m[0][0] = cos(radians(d))
    m[0][1] = -sin(radians(d))
    m[1][1] = sin(radians(d))
    m[1][2] = cos(radians(d))
    
    return m

#==============================================================

def pytGetRotationMatrixY(d):
    """ Get the rotation matrix 
    for y degrees of rotation along
    the Y axis
    
    @param d: Degrees of rotation 
    @return: Rotation matrix
    """
    m = pytGetIdentityMatrix()
    m[0][0] = cos(radians(d))
    m[0][1] = sin(radians(d))
    m[2][0] = -sin(radians(d))
    m[2][2] = cos(radians(d))
    
    return m
    
#==============================================================

def pytGetRotationMatrixX(d):
    """ Get the rotation matrix 
    for x degrees of rotation along
    the X axis
    
    @param d: Degrees of rotation 
    @return: Rotation matrix
    """
    m = pytGetIdentityMatrix()
    m[1][2] = cos(radians(d))
    m[1][3] = -sin(radians(d))
    m[2][1] = sin(radians(d))
    m[2][2] = cos(radians(d))
    
    return m


#=================================================================

def pytGetRotationMatrixXYZ(rot):
    """ Get the rotation matrix 
    for x degrees of rotation along
    the X axis
    
    @param rot: tuple of rotations (x,y,z) 
    @return: Rotation matrix
    """
    
    x = pytGetRotationMatrixX(rot[0])
    y = pytGetRotationMatrixY(rot[1])
    z = pytGetRotationMatrixZ(rot[2])
    
    return pytMultMatrix(pytMultMatrix(x,y),z)
    
#==================================================================
 
def pytMultMatrix(m1,m2):
    """ Calculate the product of 2 matrix 
    
    @param m1: Matrix 1
    @param m2: Matrix 2 
    @return Product of m1 and m2
    """
    
    res = pytGetIdentityMatrix()
    
    for i in range(4):
        res[i][0] = m1[i][0]*m2[0][0] + m1[i][1]*m2[1][0] + m1[i][2]*m2[2][0] + m1[i][3]*m2[3][0]
        res[i][1] = m1[i][0]*m2[0][1] + m1[i][1]*m2[1][1] + m1[i][2]*m2[2][1] + m1[i][3]*m2[3][1]
        res[i][2] = m1[i][0]*m2[0][2] + m1[i][1]*m2[1][2] + m1[i][2]*m2[2][2] + m1[i][3]*m2[3][2]
        res[i][3] = m1[i][0]*m2[0][3] + m1[i][1]*m2[1][3] + m1[i][2]*m2[2][3] + m1[i][3]*m2[3][3]
        
    return res

#==================================================================

def pytVMatMult(m,v):
    """ Multiply a matrix with a vector of 4 elements
    
    @param m: 4x4 matrix
    @param v: 4 element vector 
    @return: v*m
    """
    
    res = [0,0,0,0]
    
    res[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0] + v[3]*m[3][1]
    res[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1] + v[3]*m[3][1]
    res[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2] + v[3]*m[3][2]
    res[3] = v[0]*m[0][3] + v[1]*m[1][3] + v[2]*m[2][3] + v[3]*m[3][3]
    
    return res

def transpose(m):
    result = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]
    result[0][0] = m[0][0]
    result[0][1] = m[1][0]
    result[0][2] = m[2][0]
    result[0][3] = m[3][0]

    
    result[1][0] = m[0][1]
    result[1][1] = m[1][1]
    result[1][2] = m[2][1]
    result[1][3] = m[3][1]
    
    result[2][0] = m[0][2]
    result[2][1] = m[1][2]
    result[2][2] = m[2][2]
    result[2][3] = m[3][2]

    result[3][0] = m[0][3]
    result[3][1] = m[1][3]
    result[3][2] = m[2][3]
    result[3][3] = m[2][3]
    return result
    

#==================================================================

def pytCopyMatrix(m):
    """ Get a copy of this matrix 
    
    @return Copy of the input matrix
    """
    res = pytGetIdentityMatrix()
    for i in range(4):
        res[i][0] = m[i][0]
        res[i][1] = m[i][1]
        res[i][2] = m[i][2]
        res[i][3] = m[i][3]
        
    return res


#====================================================================
#
if __name__ == '__main__':
    __m = pytGetIdentityMatrix()
    print __m
    pytTranslateMatrix(__m,0,100,0)
    print __m
    tr = transpose(__m)
    print pytMultMatrix(tr,__m);
#    __r = pytGetRotationMatrixZ(90)
#    print pytVMatMult(__r,[0,1,0,1])
